#include "Puma2C.h"

Limmitation::Limmitation() {
	x = y = w = h = 0.0f;
}

Limmitation::Limmitation(float x, float y, float w, float h) {
	this->x = x;
	this->y = y;
	this->w = w;
	this->h = h;
}

Puma2C::Puma2C(float fPrec, float sPrec) {
	angleSpace = NULL;
	this->SetPrecision(fPrec, sPrec);
	this->reinitSpaceTab();
	this->firstAngle = this->secAngle = 0.0f;
	this->firstLength = 0.3f;
	this->secondLength = 0.5f;
	memset(&remBegin, 0, sizeof(PumaPack));
	memset(&remEnd, 0, sizeof(PumaPack));
	find = sind = 0;
}

Puma2C::~Puma2C(){
	freeSpaceTab();
}

void Puma2C::GetPointsPos(float& Mx, float& My, float& Ex, float& Ey) {
	float bA;
	//int fi, si;
	//fi = (int)ceil(firstAngle/precOfFirstAngle);
	//si = (int)ceil(secAngle/precOfSecAngle);
	//if(angleSpace[si][fi]) {
		Mx = (float)sin(firstAngle)*firstLength;
		My = (float)cos(firstAngle)*firstLength;
		bA = firstAngle + secAngle;
		Ex = (float)sin(bA)*secondLength + Mx;
		Ey = (float)cos(bA)*secondLength + My;
	//} else {
	//	Mx = My = Ex = Ey = 0.0f;
	//}
}

void Puma2C::SetAngles(float first, float second) {
	firstAngle = first;
	secAngle = second;
	if(firstAngle < 0.0f)
		firstAngle += PI2;
	if(secAngle < 0.0f)
		secAngle += PI2;
}

void Puma2C::GetAngles(float* first, float* second) {
	if(first != NULL)
		*first = firstAngle;
	if(second != NULL)
		*second = secAngle;
}

void Puma2C::SetPrecision(float firstPrec, float secPrec) {
	precOfFirstAngle = firstPrec;
	precOfSecAngle = secPrec;
}

void Puma2C::GetPrecision(float *firstPrec, float *secPrec) {
	if(firstPrec != NULL)
		*firstPrec = precOfFirstAngle;
	if(secPrec != NULL)
		*secPrec = precOfSecAngle;
}

void Puma2C::SetLengths(float firstLen, float secLen) {
	firstLength = firstLen;
	secondLength = secLen;
}

void Puma2C::GetLengths(float *firstLen, float *secLen) {
	if(firstLen != NULL)
		*firstLen = firstLength;
	if(secLen != NULL)
		*secLen = secondLength;
}

void Puma2C::clearSpaceTab() {
	if(angleSpace == NULL) return;
	for(int t=0; t<sLength; ++t) {
		if(angleSpace[t] == NULL) continue;
		for(int f=0; f<fLength; ++f)
			if(angleSpace[t][f])
				angleSpace[t][f] = 1;
	}
}

void Puma2C::eraseSpaceTab() {
	if(angleSpace == NULL) return;
	for(int t=0; t<sLength; ++t) {
		if(angleSpace[t] == NULL) continue;
		for(int f=0; f<fLength; ++f)
				angleSpace[t][f] = 1;
	}
}

void Puma2C::freeSpaceTab() {
	if(angleSpace == NULL) return;
	for(int t=0; t<sLength; ++t) {
		if(angleSpace[t] == NULL) continue;
		delete [] angleSpace[t];
		angleSpace[t] = NULL;
	}
	delete [] angleSpace;
	angleSpace = NULL;
}

void Puma2C::reinitSpaceTab() {
	freeSpaceTab();
	fLength = (int)ceil(PI2/precOfFirstAngle);
	sLength = (int)ceil(PI2/precOfSecAngle);
	angleSpace = new int*[sLength];
	if(angleSpace == NULL)
		throw "RunOutOfMemory";

	for(int t=0; t<sLength; ++t) {
		angleSpace[t] = NULL;
		angleSpace[t] = new int[fLength];
		for(int tt=0; tt<fLength; ++tt)
			angleSpace[t][tt] = 1;
		if(angleSpace[t] == NULL)
			throw "RunOutOfMemory";
	}
}

void Puma2C::ClearLimmitations(deque<Pos>* deq) {
	limmits.clear();
	eraseSpaceTab();
	CheckSpace(deq);
}

void Puma2C::SetSpaceTabSize(int fSize, int sSize) {
	precOfFirstAngle = PI2/(float)fSize;
	precOfSecAngle = PI2/(float)sSize;
	reinitSpaceTab();
}

float Puma2C::CheckIndex(int f, int s, Pos p) {
	Pos m,e;

	firstAngle = f * precOfFirstAngle;
	secAngle = s * precOfSecAngle;
	GetPointsPos(m.x, m.y, e.x, e.y);
	float x = e.x - p.x;
	float y = e.y - p.y;
	return x*x + y*y;
}

bool Puma2C::AngleValue(PumaPack& sol) {
	int fi[3];
	int si[3];
	Pos ram, sm;
	float d, d2 = 100.0f;
	int fii = 0, sii = 0;
	if(sol.fang < 0) sol.fang += PI2;
	if(sol.sang < 0) sol.sang += PI2;

	fi[0] = ((int)(sol.fang / precOfFirstAngle))%fLength;
	si[0] = ((int)(sol.sang / precOfSecAngle))%sLength;
	fi[1] = (fi[0] - 1 + fLength)%fLength;
	si[1] = (si[0] - 1 + sLength)%sLength;
	fi[2] = (fi[0] + 1 + fLength)%fLength;
	si[2] = (si[0] + 1 + sLength)%sLength;

	firstAngle = sol.fang;
	secAngle = sol.sang;
	GetPointsPos(sm.x, sm.y, ram.x, ram.y);
	for(int f=0; f<3; ++f) {
		for(int s=0; s<3; ++s) {
			d = CheckIndex(fi[f], si[s], ram);
			if(d < d2) {
				d2 = d;
				fii = f;
				sii = s;
			}
		}
	}
	sol.fang = fi[fii] * precOfFirstAngle;
	sol.find = fi[fii];
	sol.sang = si[sii] * precOfSecAngle;
	sol.sind = si[sii];
	sol.aviable = angleSpace[si[sii]][fi[fii]];

	return angleSpace[si[sii]][fi[fii]];
}

bool Puma2C::Reverse(float x, float y, 
					 PumaPack& sol, PumaPack& sol2) {
	float x2 = x * x;
	float y2 = y * y;
	float l1 = firstLength, l2 = secondLength;
	float l12 = firstLength * firstLength;
	float l22 = secondLength * secondLength;
	float cossol = (x2 + y2 - l12 - l22)
		/(-2.0f * firstLength * secondLength); 
	if(cossol > 1.0f) cossol = 1.0f;
	if(cossol < -1.0f) cossol = -1.0f;
	sol.sang = acosf( cossol );
	sol.sang = PI + sol.sang;
	sol2.sang = -sol.sang;

	cossol = (l22 - l12 - x2 - y2) / (-2.0f * firstLength * (float)sqrt(x2 + y2));
	if(cossol > 1.0f) cossol = 1.0f;
	if(cossol < -1.0f) cossol = -1.0f;
	float alfa = acosf( cossol );
	float beta = (float)atan2( x, y );
	sol.fang = beta + alfa;
	sol2.fang = beta - alfa;
	AngleValue(sol);
	AngleValue(sol2);
	if(!sol.aviable && sol.choosen && sol2.aviable) { sol2.choosen = true; sol.choosen = false; }
	if(!sol2.aviable && sol2.choosen && sol.aviable) { sol.choosen = true; sol2.choosen = false; }
	return sol.aviable || sol2.aviable;
}

void Puma2C::UpdateLimit(float x, float y, deque<Pos>* deq) {
	Limmitation l = limmits.back();
	l.w = x - l.x;
	l.h = y - l.y;
	//l.y = max(y, l.y);
	//l.x = min(x, l.x);
	limmits.back() = l;
}

void Puma2C::UpdatePoints(deque<Pos>* deq) {
	Limmitation lim = limmits.back();
	float fAng, sAng;
	Pos p;

	for(int s=0; s<sLength; ++s) {
		for(int f=0; f<fLength; ++f) {
			if(!angleSpace[s][f]) continue;
			fAng = (float)f * precOfFirstAngle;
			sAng = (float)s * precOfSecAngle;
			if(deq != NULL) {
				p = deq->front();
				deq->pop_front();
			}
			if(!CrossLimmitation(lim, fAng, sAng)) {
				angleSpace[s][f] &= -1;
				if(deq != NULL)
					deq->push_back(p);
			}
			else 
				angleSpace[s][f] &= 0;
		}
	}
}

void Puma2C::AddLimit(float x, float y, float w, float h, deque<Pos>* deq) {
	Limmitation lim(x,y,w,h);
	limmits.push_back(lim);

	/*for(int s=0; s<sLength; ++s) {
		for(int f=0; f<fLength; ++f) {
			if(!angleSpace[s][f]) continue;
			fAng = (float)f * precOfFirstAngle;
			sAng = (float)s * precOfSecAngle;
			if(deq != NULL) {
				p = deq->front();
				deq->pop_front();
			}
			if(!CrossLimmitation(lim, fAng, sAng)) {
				angleSpace[s][f] &= -1;
				if(deq != NULL)
					deq->push_back(p);
			}
			else 
				angleSpace[s][f] &= 0;
		}
	}*/
}

bool Puma2C::CrossLimmitation(const Limmitation& limit, float fAng, float sAng) {
	// M - middle, E-effector
	bool cross = true;
	float Mx, Ex, My, Ey, bA;
	Mx = (float)sin(fAng)*firstLength;
	My = (float)cos(fAng)*firstLength;
	bA = fAng + sAng;
	Ex = (float)sin(bA)*secondLength + Mx;
	Ey = (float)cos(bA)*secondLength + My;

	//drugie ramie robota
	if(CheckIfCross(Mx, My, Ex, Ey, limit.x, limit.y, limit.x + limit.w, limit.y)) 
		return true;
	if(CheckIfCross(Mx, My, Ex, Ey, limit.x + limit.w, limit.y, limit.x + limit.w, limit.y + limit.h))
		return true;
	if(CheckIfCross(Mx, My, Ex, Ey, limit.x + limit.w, limit.y + limit.h, limit.x, limit.y + limit.h))
		return true;
	if(CheckIfCross(Mx, My, Ex, Ey, limit.x, limit.y + limit.h, limit.x, limit.y))
		return true;

	//pierwsze ramie robota
	if(CheckIfCross(Mx, My, 0, 0, limit.x, limit.y, limit.x + limit.w, limit.y))
		return true;
	if(CheckIfCross(Mx, My, 0, 0, limit.x + limit.w, limit.y, limit.x + limit.w, limit.y + limit.h))
		return true;
	if(CheckIfCross(Mx, My, 0, 0, limit.x + limit.w, limit.y + limit.h, limit.x, limit.y + limit.h))
		return true;
	if(CheckIfCross(Mx, My, 0, 0, limit.x, limit.y + limit.h, limit.x, limit.y))
		return true;

	return false;
}

bool Puma2C::Include(float a, float b, float x) {
	float tmp;
	if(a > b) {
		tmp = a;
		a = b;
		b = tmp;
	}
	if(x >= a - EPS && x <= b + EPS) return true;
	return false;
}
/*******************************************
 * x1,y1 ,x2,y2 - punkty robota
 * x3,y3, x4,y4 - punkty sciany ograniczenia
 *******************************************/
bool Puma2C::CheckIfCross(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
	float A1, B1, C1, A2, B2, C2, x, y;
	CalculateLine(x1, y1, x2, y2, A1, B1, C1);
	CalculateLine(x3, y3, x4, y4, A2, B2, C2);
	if(!CalculateCrossPoint(A1, B1, C1, A2, B2, C2, x, y))
		return false;	
	return Include(x1, x2, x) && Include(x3, x4, x) && Include(y1, y2, y) && Include(y3, y4, y);
}

void Puma2C::CalculateLine(float x1, float y1, float x2, float y2, float &A, float &B, float &C) {
	//(x - x1) / (x2 - x1) = (y - y1) / (y2 - y1);
	//x (y2 - y1) - x1 (y2 - y1)  = y (x2 - x1) - y1 (x2 - x1) ;
	//x (y2 - y1) + y ( -x2 + x1)  + y1 (x2 - x1) - x1 (y2 - y1)  = 0 ;
	A = y2 - y1;
	B = x1 - x2;
	C = y1 * (x2 - x1) - x1 * (y2 - y1);
}

bool Puma2C::CalculateCrossPoint(float A1, float B1, float C1, float A2, float B2, float C2, float& X, float& Y) {
	float Wab, Wbc, Wca;
	Wab = A1*B2 - A2*B1;
	Wbc = B1*C2 - B2*C1;
	Wca = C1*A2 - C2*A1;
	if(Wab >= -EPS && Wab <= EPS)
		return false;
	X = Wbc/Wab;
	Y = Wca/Wab;
	return true;
}

void Puma2C::CheckSpace(std::deque<Pos> *deq) {
	float fAng, sAng;
	Limmitation lim;
	bool av;
	Pos p, ps;

	deq->clear();
	/*for(unsigned int i=0; i<limmits.size(); ++i) {
		lim = limmits.front();
		limmits.pop_front();
		limmits.push_back(lim);

		for(int s=0; s<sLength; ++s) {
			for(int f=0; f<fLength; ++f) {
				fAng = (float)f * precOfFirstAngle;
				sAng = (float)s * precOfSecAngle;
				av = CrossLimmitation(lim, fAng, sAng);
				if(!av) {
					angleSpace[s][f] &= -1;
				}
				else 
					angleSpace[s][f] &= 0;
			}
		}
	}
*/
	for(int s=0; s<sLength; ++s) {
		for(int f=0; f<fLength; ++f) {
			if(angleSpace[s][f] == 0)
				continue;

			fAng = (float)f * precOfFirstAngle;
			sAng = (float)s * precOfSecAngle;
			firstAngle = fAng;
			secAngle = sAng;
			GetPointsPos(ps.x, ps.y, p.x, p.y);
			deq->push_back(p);
		}
	}
}

void Puma2C::CrossLimmitation(float fAng, float sAng) {
	Limmitation lim;
	Pos p, ps;

	for(unsigned int i=0; i<limmits.size(); ++i) {
		lim = limmits.front();
		limmits.pop_front();
		limmits.push_back(lim);

		CrossLimmitation(lim, fAng, sAng);
	}
}

bool Puma2C::FindPath(PumaPack beg, PumaPack end) {
	remBegin = beg;
	remEnd = end;
	return FindPath();
}

bool Puma2C::FindPath() {
	int tf[3];
	int ts[3];
	FindStr fin;
	deque<FindStr> stos;

	clearSpaceTab();
	fin.f = remEnd.find;
	fin.s = remEnd.sind;
	fin.val = -1;
	angleSpace[fin.s][fin.f] = -1;
	stos.push_back(fin);

	while(!stos.empty()) {
		fin = stos.back();
		stos.pop_back();
		--fin.val;
		tf[0] = (fin.f - 1 + fLength)%fLength;
		tf[1] = fin.f;
		tf[2] = (fin.f + 1)%fLength;

		ts[0] = (fin.s - 1 + sLength)%sLength;
		ts[1] = fin.s;
		ts[2] = (fin.s + 1)%sLength;
		for(int sii=0; sii<3; ++sii) {
			for(int fii=0; fii<3; ++fii) {
				if(angleSpace[ts[sii]][tf[fii]] > 0
					|| angleSpace[ts[sii]][tf[fii]] < fin.val	) {
					fin.s = ts[sii]; fin.f = tf[fii];
					angleSpace[ts[sii]][tf[fii]] = fin.val;
					stos.push_front(fin);
				}
			}
		}
	}
	find = remBegin.find;
	sind = remBegin.sind;
	return true;
}

void Puma2C::NextStep(float& fang, float& sang) {
	if(remEnd.find == find && remEnd.sind == sind) {
		find = remBegin.find;
		sind = remBegin.sind;
		return;
	}
	int cur = angleSpace[sind][find];
	int t1, t2, d, dmin = 100000;
	int tf[3];
	int ts[3];

	tf[0] = (find - 1 + fLength)%fLength;
	tf[1] = find;
	tf[2] = (find + 1)%fLength;

	ts[0] = (sind - 1 + sLength)%sLength;
	ts[1] = sind;
	ts[2] = (sind + 1)%sLength;
	
	for(int sii=0; sii<3; ++sii) {
		for(int fii=0; fii<3; ++fii) {
			if( angleSpace[ts[sii]][tf[fii]] >= cur 
				&& angleSpace[ts[sii]][tf[fii]] ) {
					t1 = ts[sii]-remEnd.sind;
					t2 = tf[fii]-remEnd.find;
					d =  t1*t1 + t2*t2;
				if(dmin > d) {
					dmin = d;
					sind = ts[sii]; find = tf[fii];
					cur = angleSpace[ts[sii]][tf
[fii]];
				}
			}
		}
	}
	fang = (float)find * precOfFirstAngle;
	sang = (float)sind * precOfSecAngle;
}